%===================================================================================================
%[]FUNCTION NAME: LambertTi.m
%[]AUTHOR: Jinsung Lee
%[]CREATED: 09/01/2017
%===================================================================================================
%[]FUNCTION DESCRIPTION:
%This function calculates the departure and arrival velocity vectors needed to transfer a satellite
%between two (2) points in space using a minimum delta-v trajectory.
%===================================================================================================
%[]INPUT VARIABLES:
%(R1)|Initial position.
%---------------------------------------------------------------------------------------------------
%(V1minus)|Initial orbital velocity.
%---------------------------------------------------------------------------------------------------
%(R2)|Final position.
%---------------------------------------------------------------------------------------------------
%(V2plus)|Final orbital velocity.
%---------------------------------------------------------------------------------------------------
%(u)|Central body gravitational parameter.
%===================================================================================================
%[]OUTPUT VARIABLES:
%(V1plus)|Departure velocity vector.
%---------------------------------------------------------------------------------------------------
%(V2minus)|Arrival velocity vector.
%---------------------------------------------------------------------------------------------------
%(dt)|Time of flight.
%===================================================================================================
%[]VARIABLE FORMAT:
%(R1)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(V1minus)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(R2)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(V2plus)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(u)|Scalar {1 x 1}.
%---------------------------------------------------------------------------------------------------
%(V1plus)|Column Vector {3 x 1}.
%---------------------------------------------------------------------------------------------------
%(V2minus)|Column Vector {3 x 1}.
%===================================================================================================
%[]AUXILIARY FUNCTIONS:
%(Rotation.m)|This function calculates the matrix needed to actively rotate a vector about an axis
%by a given angle using the Rodrigues rotation formula.
%===================================================================================================
%[]COMMENTS:
%None.
%===================================================================================================
function [V1plus,V2minus,dt,dv] = LambertTi_single(R1,V1minus,R2,V2plus,u)

r1 = norm(R1);
%[km]Initial distance from the central body.

r2 = norm(R2);
%[km]Final distance from the central body.

sigma = r1 / r2;
%[]Ratio of the orbital ranges.

phi = acos(dot(R1,R2) / (r1 * r2));
%[rad]Transfer angle.

W = cross(R1,R2) / norm(cross(R1,R2));
%[]Third direction in the perifocal coordinate system.

if W(3) < 0
    
    phi = 2 * pi - phi;
    %[rad]Transfer angle correction.
    
    W = -W;
    %[]Third direction in the PQW coordinate system correction.
    
end

emin = (1 - sigma) / sqrt(sigma^2 - 2 * sigma * cos(phi) + 1);
%[]Minimum eccentricity for this transfer.

Phase = atan2(sigma - cos(phi),sin(phi));
%[rad]Phase angle.


if r1 < r2;
    
    theta = 0;
    %[rad]True anomaly range.
    
else
    
    theta = pi;
    %[rad]True anomaly range.
    
end

e = emin / sin(theta + Phase);
%[rad]Current eccentricity.

p = r1 * (1 + e * cos(theta));
%[km]Current semiparameter.

Rot = Rotation(W,-theta,'Radians');
%[]Matrix that rotates a vector about the W-axis by an angle -theta.

P = Rot * R1 / r1;
%[]Principle direction in the perifocal coordinate system.

Q = cross(W,P);
%[]Secondary direction in the perifocal coordinate system.

PqwToEci = [P, Q, W];
%[]Matrix needed to transform vectors from PQW coordinates to ECI coordiantes.

V1plus = sqrt(u / p) * PqwToEci * [-sin(theta); cos(theta) + e; 0];
%[km/s]Current departure velocity.

V2minus = sqrt(u / p) * PqwToEci * [-sin(theta + phi); cos(theta + phi) + e; 0];
%[km/s]Current arrival velocity.

dv1 = norm(V1plus - V1minus);
%[km/s]Current departure change in speed.

dv2 = norm(V2plus - V2minus);
%[km/s]Current arrival change in speed.

dv = dv1 + dv2;
%[km/s]Current total mission change in speed.

e = emin / sin(theta + Phase);
%[]Optimal transfer orbit eccentricity.

p = r1 * (1 + e * cos(theta));
%[km]Current semiparameter.

E1 = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(theta / 2));
%[rad]Optimal transfer orbit departure eccentric anomaly.

E2 = 2 * atan(sqrt((1 - e) / (1 + e)) * tan((theta + phi) / 2));
%[rad]Optimal transfer orbit arrival eccentric anomaly.

a = p / (1 - e^2);
%[km]Optimal transfer orbit semimajor axis.

n = sqrt(u / a^3);
%[rad/s]Optimal transfer orbit mean angular motion.

t1 = (E1 - e * sin(E1)) / n;
%[s]Optimal transfer orbit departure time.

t2 = (E2 - e * sin(E2)) / n;
%[s]Optimal transfer orbit arrival time.

dt = t2 - t1;
%[s]Optimal transfer time of flight.

if dt < 0;
    
    T = 2 * pi / n;
    %[s]Optimal transfer orbital period.
    
    dt = dt + T;
    %[s]Time of flight correction.
    
end

end
%===================================================================================================